package com.lego.jbv;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;

public class NQC {
	public static void main (String[] aArg)
	throws Exception
	{
		final int tooClose = 10; 
		final int close = 20;
		final int aBitClose = 25;

		final int aBitFar = 55;
		final int far = 60;
		final int tooFar = 70;
		final int normal = 80, high = 3, mid = 2, low = 1;
		
		
		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S4);
		
		DataLogger data = new DataLogger("opgave6.csv");
	    data.start();
		
		LCD.drawString("Distance: ", 0, 1);
		LCD.drawString("Power Left: ", 0, 2);
		LCD.drawString("Power Right: ", 0, 3);

		int distance = us.getDistance(),powerLeft = normal, powerRight = normal, dDistance = 0, previousDistance = 0 ;
		while (!Button.ESCAPE.isDown())
	     {
			distance = us.getDistance();
			dDistance = previousDistance - distance;
			previousDistance = distance;
		
			if(dDistance > 100)
			{
				powerLeft = normal;
				powerRight = normal;
			}
			
			if(distance <= aBitClose)
			{
				if(distance <= tooClose)
				{
					powerLeft += high;
					powerRight -= high;
				}
				
				else if(distance <= close)
				{
					powerLeft += mid;
					powerRight -= mid;
				}
				
				else if (distance <= aBitClose)
				{
					powerLeft += low;
					powerRight -= low;
				}
				
			}
			else if(distance >= aBitFar)
			{

				if(distance >= tooFar)
				{
					powerLeft -= low;
					powerRight += low;
				}
				
				else if(distance >= far)
				{
					powerLeft -= low;
					powerRight += low;
				}
				
				else if(distance >= aBitFar)
				{
					powerLeft -= low;
					powerRight += low;
				}
			
			}
			else
			{
				powerLeft = normal;
				powerRight = normal;
				
			}
			
			if(powerLeft > 100)
			{
				powerLeft = 100;
			}
			else if(powerLeft < 0)
			{
				powerLeft = 0;
			}
			
			if(powerRight > 100)
			{
				powerRight = 100;
			}
			else if(powerRight < 0)
			{
				powerRight = 0;
			}
			
			LCD.drawInt(distance,3, 13, 1);
			LCD.drawInt(powerLeft,3, 13, 2);
			LCD.drawInt(powerRight,3, 13, 3);
			
			Car.forward(powerLeft, powerRight);
			data.writeSample(distance);
			Thread.sleep(50);
	     }
		
		data.close();
	}
}